/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */

/*
 * IMPORTAINT: Drive configuration: Top Left = 1 Top Right = 2 Bottom Left = 3
 * Bottom Right = 4
 *
 * Declarations of variables are listed below: Joysticks, drive loadout,
 * compressor, and the solenoids.
 */
public class SimpleTestBot extends SimpleRobot {

    //RobotDrive drive = new RobotDrive(1, 3, 2, 4);
    // Joystick leftStick = new Joystick(1);     
    // Joystick rightStick = new Joystick(2);               
    LogitechController controller = new LogitechController(1);
//    Compressor compressor = new Compressor(9, 1);
//    Solenoid pistonUp = new Solenoid(1); //Plus is forward / up
//    Solenoid pistonDown = new Solenoid(2); // Minus is backward / down
//    RateGyro gyro = new RateGyro(2);
//    double leftOffset = 0;
//    double rightOffset = 0;
    Lifter testLifter = new Lifter(5, 3); // PWM 4, Analog 3
    //Victor test = new Victor(6);

    public SimpleTestBot() {
        //       compressor.start();
        //       gyro.setSensitivity(0.030);
    }

    public void disabled() {
    }

    /*
     * static final int joystickButtonCount = 22; boolean[] rightStickButtons =
     * new boolean[ (joystickButtonCount + 1) ]; boolean[] leftStickButtons =
     * new boolean[ (joystickButtonCount + 1) ];
     *
     * static final int solenoidNumber = 2; Solenoid[] numberOfSolenoids = new
     * Solenoid[solenoidNumber];
     *
     * public RobotTemplate() {
     *
     * // Iterate over all the buttons on each joystick, setting state to false
     * for each int buttonNum = 1;	// start counting buttons at button 1 for
     * (buttonNum = 1; buttonNum <= joystickButtonCount; buttonNum++) {
     * rightStickButtons[buttonNum] = false; leftStickButtons[buttonNum] =
     * false; }
     *
     * // Iterate over all the solenoids on the robot, constructing each in
     * turn int solenoidNum = 1;	// start counting solenoids at solenoid 1 for
     * (solenoidNum = 0; solenoidNum < solenoidNumber; solenoidNum++) {
     * numberOfSolenoids[solenoidNum] = new Solenoid(solenoidNum + 1); } } //
     * digital in 9, relay 1
     */
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        for (int i = 0; i < 4; i++) {
            //          drive.drive(0.5, 0.0);  // drive 50% forward speed with 0% turn 
            //          Timer.delay(2.0);   // wait 2 seconds 
            //          drive.drive(0.0, 0.75); // drive 0% forward with 75% turn 
            //          Timer.delay(0.75);  // wait for the 90 degree turn to complete 
        }
        //      drive.drive(0.0, 0.0);   // drive 0% forward with 0% turn (stop) 
    }
    /**
     * This function is called once each time the robot enters operator control.
     */
    boolean pushedA = false;
    boolean pushedB = false;
    double spdTest = -1;
    double spdDelta = 0.001;

    public void operatorControl() {
        {
            getWatchdog().setEnabled(false);
            getWatchdog().feed();
            while (isOperatorControl() && isEnabled()) // loop during enabled teleop mode 
            {    

                getWatchdog().feed();
                //System.out.println("S = " + spdTest);
//                test.set(spdTest);
// 
//                spdTest = spdTest + spdDelta;
//
//                
//                if (spdTest > 1) {
//                    spdDelta = -0.001;
//                }
//                if (spdTest < -1) {
//                     spdDelta = 0.001;
//                }

//                if (controller.getButtonStart()) {
//                    leftOffset = controller.getLeftY();
//                    rightOffset = controller.getRightY();
//                    System.out.println("Left Offset = " + leftOffset + " Right Offset = " + rightOffset);
//                    gyro.reset();
//                }

                //drive.tankDrive(leftStick, rightStick);  // drive with the joysticks 
//                drive.tankDrive(controller.getLeftY(), controller.getRightY());
//                  drive.tankDrive(0, 0);
                Timer.delay(0.05);

                if(getWatchdog().getEnabled())  {
                    System.out.print("W");                    
                }  else {
                    System.out.print("-");
                }
                if(getWatchdog().isSystemActive()) {
                    System.out.print("A");
                } else {
                    System.out.print("-");
                }
                  
                System.out.println("PID Command - " + testLifter.getCommandedPosition() + " @ " + testLifter.getPosition() + " E: " + testLifter.getError() + " R: " + testLifter.getControlImpulse());
                

                if (controller.getButtonX()) {
                    System.out.println("Pressed X");
                    testLifter.startLifter();
                }

                if (controller.getButtonA() && !pushedA) {
                    // Goto Position A
                    testLifter.setPosition(950);
                    pushedA = true;
                    pushedB = false;
                    System.out.println("Pressed A");
                }
                if (controller.getButtonB() && !pushedB) {
                             // Goto Position B
                    testLifter.setPosition(980);
                    pushedB = true;
                    pushedA = false;
                    System.out.println("Pressed B");
                }

                //System.out.println("Position is " + testLifter.getPosition());

                //System.out.println("gyro = " + gyro.getAngle() + gyro.getAngleRate());
                /*
                 * The following code allows for the operator to control the
                 * pneumatics system with the left trigger extending the piston
                 * and the second button on the left joystick retracting the
                 * piston. Recall that the solenoids are in ports 1 and 2,
                 * respectively.
                 */

//                if (controller.getButtonA()) {
//                    pistonUp.set(false);
//                    pistonDown.set(true);
//                } else if (controller.getButtonB()) {
//                    pistonUp.set(true);
//                    pistonDown.set(false);
//                } else {
//                    pistonUp.set(false);
//                    pistonDown.set(false);
//                }
            }
        }
    }
}
